#include <SoftwareSerial.h>

// Serial communication between GPS and Arduino
SoftwareSerial GPS(4, 5);
boolean gps_set_success = 0;

char GPSchar = '\0'; // Current character from the GPS
char GPSstring[100]; // Data buffer for a gps line
unsigned int GPSpos = 0; // Current write location in the gps data buffer

const byte sentinel= B00100011;  // Indicate the end of a gps string (#)

void setup() {
  Serial.begin(115200); 

  delay(2000);
  GPSsetup();
  delay(1000);
}

void loop() {
    while(GPS.available()) {
        GPSchar = GPS.read();
        if (GPSchar == '$')
        {
          Serial.write((byte *)GPSstring, GPSpos);
          Serial.write(sentinel);
          GPSpos = 0;
        }
        GPSstring[GPSpos++] = GPSchar;
    }
}

void GPSsetup() {
  GPS.begin(9600); 
  //
  // THE FOLLOWING COMMAND SWITCHES MODULE TO 4800 BAUD
  // THEN SWITCHES THE SOFTWARE SERIAL TO 4,800 BAUD
  //
  GPS.print("$PUBX,41,1,0007,0003,4800,0*13\r\n"); 
  GPS.begin(4800);
  GPS.flush();
 
  //  THIS COMMAND SETS FLIGHT MODE AND CONFIRMS IT 
  uint8_t setNav[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 
      0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA,
      0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 
      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC};
  while(!gps_set_success) {
    sendUBX(setNav, sizeof(setNav)/sizeof(uint8_t));
    gps_set_success=getUBX_ACK(setNav);
  }
  gps_set_success=0;

  // Turning off all GPS NMEA strings apart from GPGGA on the uBlox modules
  GPS.print("$PUBX,40,GLL,0,0,0,0*5C\r\n");
  GPS.print("$PUBX,40,ZDA,0,0,0,0*44\r\n");
  GPS.print("$PUBX,40,VTG,0,0,0,0*5E\r\n");
  GPS.print("$PUBX,40,GSV,0,0,0,0*59\r\n");
  GPS.print("$PUBX,40,GSA,0,0,0,0*4E\r\n");
  GPS.print("$PUBX,40,RMC,0,0,0,0*47\r\n");
}

// Send a byte array of UBX protocol to the GPS
void sendUBX(uint8_t *MSG, uint8_t len) {
  for(int i=0; i<len; i++) {
    GPS.write(MSG[i]);
  }
  GPS.println();
}
 
// Calculate expected UBX ACK packet and parse UBX response from GPS
boolean getUBX_ACK(uint8_t *MSG) {
  uint8_t b;
  uint8_t ackByteID = 0;
  uint8_t ackPacket[10];
  unsigned long startTime = millis();
 
  // Construct the expected ACK packet    
  ackPacket[0] = 0xB5;	// header
  ackPacket[1] = 0x62;	// header
  ackPacket[2] = 0x05;	// class
  ackPacket[3] = 0x01;	// id
  ackPacket[4] = 0x02;	// length
  ackPacket[5] = 0x00;
  ackPacket[6] = MSG[2];	// ACK class
  ackPacket[7] = MSG[3];	// ACK id
  ackPacket[8] = 0;		// CK_A
  ackPacket[9] = 0;		// CK_B
 
  // Calculate the checksums
  for (uint8_t i=2; i<8; i++) {
    ackPacket[8] = ackPacket[8] + ackPacket[i];
    ackPacket[9] = ackPacket[9] + ackPacket[8];
  }
 
  while (1) {
    // Test for success
    if (ackByteID > 9) {
      // All packets in order!
      return true;
    }
 
    // Timeout if no valid response in 3 seconds
    if (millis() - startTime > 3000) { 
      return false;
    }
 
    // Make sure data is available to read
    if (GPS.available()) {
      b = GPS.read();
 
      // Check that bytes arrive in sequence as per expected ACK packet
      if (b == ackPacket[ackByteID]) { 
        ackByteID++;
      } 
      else {
        ackByteID = 0;	// Reset and look again, invalid order
      }
    }
  }
}
